#include <algorithm>
#include <iostream>
#include <atomic>
#include "CanDriver.hpp"
using sockcanpp::CanDriver;
using sockcanpp::CanMessage;

uint8_t dm_enable_data[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc};
uint8_t dm_disable_data[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfd};
int16_t amp_averge = 800;
uint8_t amp_averge_high = amp_averge >> 8;
uint8_t amp_averge_low = amp_averge & 0xff;
uint8_t dji_averge_data[8] = {amp_averge_high, amp_averge_low, amp_averge_high, amp_averge_low, 0x00, 0x00, 0x00, 0x00};
int16_t amp_uplift = 1000;
uint8_t amp_uplift_high = amp_uplift >> 8;
uint8_t amp_uplift_low = amp_uplift & 0xff;
uint8_t dji_uplift_data[8] = {amp_uplift_high, amp_uplift_low, amp_uplift_high, amp_uplift_low, 0x00, 0x00, 0x00, 0x00};
int16_t amp_down = 600;
uint8_t amp_down_high = amp_down >> 8;
uint8_t amp_down_low = amp_uplift & 0xff;
uint8_t dji_down_data[8] = {amp_down_high, amp_down_low, amp_down_high, amp_down_low, 0x00, 0x00, 0x00, 0x00};

can_frame dm1_enable;
can_frame dm2_enable;
can_frame dm1_disable;
can_frame dm2_disable;
can_frame dji_averge_frame;
can_frame dji_uplift_frame;
can_frame dji_down_frame;

void initFrame() {
    dm1_enable.can_id = 0x151;
    dm1_enable.len = 8;
    std::copy_n(dm_enable_data, 8, dm1_enable.data);
    dm2_enable.can_id = 0x152;
    dm2_enable.len = 8;
    std::copy_n(dm_enable_data, 8, dm2_enable.data);

    dm1_disable.can_id = 0x151;
    dm1_disable.len = 8;
    std::copy_n(dm_disable_data, 8, dm1_disable.data);
    dm2_disable.can_id = 0x152;
    dm2_disable.len = 8;
    std::copy_n(dm_disable_data, 8, dm2_disable.data);

    dji_averge_frame.can_id = 0x200;
    dji_averge_frame.len = 8;
    std::copy_n(dji_averge_data, 8, dji_averge_frame.data);

    dji_uplift_frame.can_id = 0x200;
    dji_uplift_frame.len = 8;
    std::copy_n(dji_uplift_data, 8, dji_uplift_frame.data);

    dji_down_frame.can_id = 0x200;
    dji_down_frame.len = 8;
    std::copy_n(dji_down_data, 8, dji_down_frame.data);

}

std::atomic<int> choice(1);

void sendFeedback1(CanDriver& canDriver) {
    while (true) {
        if (choice == 0) {
            canDriver.sendMessage(CanMessage(dm1_disable));
            canDriver.sendMessage(CanMessage(dm2_disable));
            break;
        }
        if (choice == 1) {
            canDriver.sendMessage(CanMessage(dji_down_frame));
        } else if (choice == 2) {
            canDriver.sendMessage(CanMessage(dji_averge_frame));
        } else if (choice == 3) {
            canDriver.sendMessage(CanMessage(dji_uplift_frame));
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
}

void getUserInput() {
    int input;
    while (true) {
        std::cin >> input;
        if (input == 0 || input == 1 || input == 2 || input == 3) {
            choice = input;
            if (input == 0) break;
        } else {
            std::cout << "Invalid input. Please enter 0, 1, 2 or 3." << std::endl;
        }
    }
}


int main() {
    initFrame();
    CanDriver canDriver("can0", CAN_RAW);
    canDriver.sendMessage(CanMessage(dm1_enable));
    canDriver.sendMessage(CanMessage(dm2_enable));

    std::thread sender(sendFeedback1, std::ref(canDriver));
    std::thread inputHandler(getUserInput);

    sender.join();
    inputHandler.join();

    return 0;
}